﻿////////////////////////////////////////////////////////////////
///
/// File: ExpContent.cs
/// Author: Lucio Davide Spano
/// Last update: 27 July 2012
/// Description: This class interfaces the Kinect platform with
///              the temporal expression execution engine
/// To do: 
/// ------------------------------------------------------------
/// Copyright (c) 2012 Lucio Davide Spano 
///                     (lucio.davide.spano@isti.cnr.it)
/// 
/// The use and distribution terms for this software are 
/// contained in the file named license.txt, which can be found 
/// in the root of this distribution.
/// By using this software in any fashion, you are agreeing to 
/// be bound by the terms of this license.
///
/// You must not remove this notice, or any other, from this
/// software.
/// ------------------------------------------------------------
////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

using GestIT;

namespace BodyGestIT
{
    public class BodyEmitter : Emitter
    {
        public const int skeletonCount = 6;

        private KinectSensor sensor;
        private Skeleton[] allSkeletons = new Skeleton[skeletonCount];
        private ClonableSkeleton[] clones = new ClonableSkeleton[skeletonCount];
        private bool[] leftHands = new bool[skeletonCount];
        private bool[] rightHands = new bool[skeletonCount];
        private bool handOpenTracking = false;
        private HandRecognizer recognizer;

        public BodyEmitter(KinectSensor sensor, bool handOpenTracking)
            : base()
        {
            this.handOpenTracking = handOpenTracking;
            this.sensor = sensor;

            for (int i = 0; i < skeletonCount; i++)
            {
                clones[i] = new ClonableSkeleton();
            }
            if (handOpenTracking)
            {
                try
                {
                    recognizer = new HandRecognizer();
                }
                catch (Exception e)
                {
                    Console.WriteLine(e.StackTrace);
                }
            }
            sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(sensor_AllFramesReady);
        }

        public BodyEmitter(KinectSensor sensor)
            : this(sensor, false)
        {

        }

        void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {

            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    return;
                }

                if (handOpenTracking)
                {
                    recognizer.ResetHandPoints();
                }

                skeletonFrameData.CopySkeletonDataTo(allSkeletons);
                int i = 0;
                foreach (Skeleton skeleton in allSkeletons)
                {
                    i = skeleton.TrackingId % skeletonCount;

                    if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                    {

                        clones[i].UpdateFromSkeleton(skeleton);

                        if (handOpenTracking)
                        {
                            SkeletonPoint rx = skeleton.Joints[Microsoft.Kinect.JointType.HandRight].Position;
                            SkeletonPoint lx = skeleton.Joints[Microsoft.Kinect.JointType.HandLeft].Position;
                            DepthImagePoint depthLx =
                                   sensor.MapSkeletonPointToDepth(
                                       lx,
                                       DepthImageFormat.Resolution640x480Fps30);
                            DepthImagePoint depthRx =
                                sensor.MapSkeletonPointToDepth(
                                    rx,
                                    DepthImageFormat.Resolution640x480Fps30);
                            recognizer.SetHandPoint(skeleton.TrackingId, depthLx.X, depthLx.Y, JointType.HandLeft);
                            recognizer.SetHandPoint(skeleton.TrackingId, depthRx.X, depthRx.Y, JointType.HandRight);
                            clones[i].SetHandOpen(
                                recognizer.IsOpen(skeleton.TrackingId, JointType.HandLeft),
                                recognizer.IsOpen(skeleton.TrackingId, JointType.HandRight));
                            
                        }

                        BodyEventArgs args = new BodyEventArgs(clones[i]);
                        bool feed = false;
                        foreach(SimpleTmpExp leaf in leafs)
                        {
                            // raise the events only for the registered features
                            // that changed
                            BodyContent content = (BodyContent) leaf.Content;
                            if (leaf.State == ExecState.Enabled &&
                                args.Skeleton.JointChanged[(int) content.Feature])
                            {
                                feed = true;
                                break;
                            }
                        }

                        if (feed)
                        {
                            //Console.WriteLine("feed ");
                            FeedDeviceEvent(args);
                        }
                    }

                }

                
            }
            if (handOpenTracking)
            {
                using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
                {
                    recognizer.ProcessDepthImageFrame(depthFrame);
                }
            }
        }





    }
}
